#include "loc_core.h"

#include <iostream>
#include <vector>

using std::cerr;
using std::endl;
using std::vector;

static Area area_0;
static Area area_1;
static Area area_2;
static int area_set = 0;
static Locator locator;

bool init_senario(const int n_st, const double *st, const double *a_vtx, const int *a_mask, const double * st_base, const double *a_h)
{
    // init areas
    area_0.init(a_vtx + 0, 4, a_mask + 0,       n_st, a_h[0]);
    area_1.init(a_vtx + 0, 4, a_mask + n_st*1,  n_st, a_h[0]);
    area_2.init(a_vtx + 8, 4, a_mask + n_st*2,  n_st, a_h[1]);

    // init locator
    locator.init(st, st_base, n_st);

    return true;
}

bool update_senario(const double *dist_in, const int *dist_mask, const int dist_num, double gnss_east, double gnss_north, double *e, double *n, int *area)
{
    double east, north, loss;
    bool ret = false;

    // std::cerr << "In Area " << locator.area_set << std::endl;

    if(locator.area_set == 0)
    {
        
        locator.set_area(area_0, 0);
        locator.update_dist(dist_in, dist_mask, dist_num);
        //bool is_valid = locator.solve(east, north, loss);
        if(area_1.is_in_area(gnss_east, gnss_north))
        {
            locator.set_area(area_1, 1);
            std::cerr << "Switch to area 1!" << std::endl;
            //std::cerr << "Locator report " << locator.area_set << std::endl;
        }
    }

    if(locator.area_set == 1)
    {
        locator.update_dist(dist_in, dist_mask, dist_num);
        bool is_valid = locator.solve(east, north, loss);
        if(area_2.is_in_area(east, north))
        {
            locator.set_area(area_2, 2);
            std::cerr << "Switch to area 2!" << std::endl;
            //std::cerr << "Locator report " << locator.area_set << std::endl;
        }
        ret = true && is_valid;
    }

    if(locator.area_set == 2)
    {
        locator.update_dist(dist_in, dist_mask, dist_num);
        bool is_valid = locator.solve(east, north, loss);
        if(!area_2.is_in_area(east, north))
        {
            locator.set_area(area_0, 0);
            std::cerr << "Switch to area 0!" << std::endl;
        }
        ret = true && is_valid;
    }

    *e = east;
    *n = north;
    *area = locator.area_set;

    return ret;
}